package com.jmex.physics.impl.jbullet;

import javax.vecmath.Vector3f;

import com.bulletphysics.BulletGlobals;
import com.bulletphysics.collision.dispatch.CollisionObject;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.RigidBodyConstructionInfo;
import com.bulletphysics.linearmath.MotionState;
import com.bulletphysics.linearmath.Transform;
import com.jmex.physics.Joint;
import com.jmex.physics.impl.jbullet.util.VecmathConverter;

public class NodeLockedRigidBody extends RigidBody {

	private JBulletPhysicsNode node;
	
	public NodeLockedRigidBody(JBulletPhysicsNode node, RigidBodyConstructionInfo constructionInfo) {
		super(constructionInfo);
		this.node=node;
	}

	public NodeLockedRigidBody(JBulletPhysicsNode node, float mass, MotionState motionState, CollisionShape collisionShape) {
		this(node, mass, motionState, collisionShape, BulletGlobals.ZERO_VECTOR3);
	}
	
	public NodeLockedRigidBody(JBulletPhysicsNode node, float mass, MotionState motionState, CollisionShape collisionShape, Vector3f localInertia) {
		super(mass,motionState,collisionShape,localInertia);
		this.node=node;
	}
	
	public Transform getWorldTransform() {
		Transform t = new Transform();
		t.setIdentity();
		t.origin.set(VecmathConverter.convert(node.getWorldTranslation()));
		t.setRotation(VecmathConverter.convert(node.getWorldRotation()));
 		return t;
	}
	
	public void setWorldTransform(Transform worldTrans) {
		if(this.isStaticObject() || this.isKinematicObject())
			return;
		if(node.getParent()!=null)
		{
			node.setLocalRotation(VecmathConverter.convert(worldTrans.getRotation()).mult(node.getParent().getWorldRotation().inverse()),true);
			node.setLocalTranslation(VecmathConverter.convert(worldTrans.origin).subtract(node.getParent().getWorldTranslation()),true);
		} else {
			node.setLocalRotation(VecmathConverter.convert(worldTrans.getRotation()),true);
			node.setLocalTranslation(VecmathConverter.convert(worldTrans.origin),true);
		}
		node.updateWorldVectors();
		this.worldTransform = getWorldTransform();
	}

	@Override
	public boolean checkCollideWithOverride(CollisionObject co) {
		if(co instanceof NodeLockedRigidBody)
		{
			NodeLockedRigidBody rb = (NodeLockedRigidBody)co;
			if(rb.getNode() instanceof JBulletDynamicPhysicsNode)
			{
				for(Joint joint : ((JBulletDynamicPhysicsNode)rb.getNode()).getMyJoints())
				{
					if(joint.getNodes().contains(node))
						return joint.isCollisionEnabled();
				}
			}
		}
		return super.checkCollideWithOverride(co);
	}

	public JBulletPhysicsNode getNode() {
		return node;
	}
}
